﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework;

namespace StaticView.Algorithm
{
    static class QuaternionHelper
    {
        public static Quaternion QuaternionFromTwoVectors(Vector3 a1, Vector3 b1, Vector3 a2, Vector3 b2)
        {
            a1.Normalize();
            b1.Normalize();
            a2.Normalize();
            b2.Normalize();

            Vector3 NormalA = a2 - a1; // Normal of A plane
            Vector3 NormalB = b2 - b1; // Normal of B plane

            // The rotation axis
            Vector3 Axis;

            // Special cases
            if (NormalA.Length() == 0) Axis = a1;
            else if (NormalB.Length() == 0) Axis = b1;

            // Solve system of equations
            Matrix S = new Matrix(3);
            Matrix D = new Matrix(new float[3] { 0, 0, 1 });
            S.setData(new float[9] {
                NormalA.X,  NormalA.Y,  NormalA.Z,  // Ax + By + Cz = 0
                NormalB.X,  NormalB.Y,  NormalB.Z,
                0,          0,          0           // u = 1
            });
            Matrix R = new Matrix(3, 1);

            // Try three times for x, y, z = 1
            for (int i = 0; i < 3; i++)
            {
                try
                {
                    // Set the current axis = 1
                    for (int j = 0; j < 3; j++)
                        S[2, j] = 0;
                    S[2, i] = 1;

                    R = S.Invert() * D; // The result
                    break; // Break if OK
                }
                catch (MatrixException) // If invalid, try again
                {
                    if (i >= 2) throw new Exception("Quaternion Helper: can't solve for the axis vector.");
                }
            }

            // The axis of the quaternion
            Axis = R.ToVector3();

            Axis.Normalize();

            // Calculate the perpendicular elements
            Vector3 Perp1, Perp2;
            if (NormalA.Length() != 0)
            {
                Perp1 = a1 - Vector3.Dot(a1, Axis) * Axis;
                Perp2 = a2 - Vector3.Dot(a2, Axis) * Axis;
            }
            else if (NormalB.Length() != 0)
            {
                Perp1 = b1 - Vector3.Dot(b1, Axis) * Axis;
                Perp2 = b2 - Vector3.Dot(b2, Axis) * Axis;
            }
            else return new Quaternion(0, 0, 0, 1);

            // Calculate the angle
            float Angle = (float)Math.Acos(Vector3.Dot(Perp1, Perp2) / (Perp1.Length() * Perp2.Length()));
            // Check the direction
            float Dir = Vector3.Dot(Axis, Vector3.Cross(Perp1, Perp2));
            if (Dir < 0) Axis = -Axis;

            return Quaternion.CreateFromAxisAngle(Axis, Angle);
        }
    }
}
